package com.robotic.test;

import lombok.Data;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Arrays;

// For command 0x1009: Robot Motion State Information
@Data
public class MotionStateData {
    private final int basicState;
    private final int gaitState;
    private final float maxForwardVel;
    private final float maxBackwardVel;
    private final float[] legOdomPos; // x, y, yaw
    private final float[] legOdomVel; // x_vel, y_vel, yaw_vel
    private final float robotDistance;
    private final int touchState;
    private final int controlState;
    private final byte[] taskState;

    public MotionStateData(byte[] data) {
        ByteBuffer buffer = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
        this.basicState = buffer.get();
        this.gaitState = buffer.get();
        buffer.getShort(); // Padding
        this.maxForwardVel = buffer.getFloat();
        this.maxBackwardVel = buffer.getFloat();
        this.legOdomPos = new float[]{buffer.getFloat(), buffer.getFloat(), buffer.getFloat()}; // 3 * 4 = 12 bytes
        this.legOdomVel = new float[]{buffer.getFloat(), buffer.getFloat(), buffer.getFloat()}; // 3 * 4 = 12 bytes
        this.robotDistance = buffer.getFloat(); // 4 bytes
        this.touchState = buffer.getInt();
        this.controlState = buffer.getInt();
        this.taskState = new byte[10];
        buffer.get(this.taskState);
    }
    public String getBasicStateDescription() {
        switch (basicState) {
            case 0: return "Lying Down";
            case 1: return "Standing Up";
            case 2: return "Initial Stand";
            case 3: return "Force-Controlled Stand";
            case 4: return "Stepping";
            case 5: return "Lying Down (in progress)";
            case 6: return "Soft Stop/Fallen";
            case 16: return "RL State";
            default: return "Unknown";
        }
    }

    public String getGaitStateDescription() {
        switch (gaitState) {
            case 0: return "Walking";
            case 1: return "Obstacle Avoidance";
            case 2: return "Slope/Non-slip";
            case 3: return "Running";
            case 6: return "Stairs (Solid/Hollow/No-riser)";
            case 7: return "Stairs (Accumulated Frame)";
            case 8: return "45-degree Stairs (Accumulated Frame)";
            case 32: return "L-Walk";
            case 33: return "Mountain";
            case 34: return "Silent";
            default: return "Unknown";
        }
    }


    @Override
    public String toString() {
        return "MotionStateData{" +
                "basicState=" + getBasicStateDescription() +
                ", gaitState=" + getGaitStateDescription() +
                ", maxForwardVel=" + maxForwardVel +
                ", maxBackwardVel=" + maxBackwardVel +
                ", legOdomPos=" + Arrays.toString(legOdomPos) +
                ", legOdomVel=" + Arrays.toString(legOdomVel) +
                ", robotDistance=" + robotDistance +
                '}';
    }
}
